%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% RPI-MATLAB-Simulator
% http://code.google.com/p/rpi-matlab-simulator/
% mlcp_fixed_point.m 
%
% MCP solver with prox function formulation with fixed point iteration 
function solution = mlcp_fixed_point( sim )
% matrices used 
%factor = sim.solver.Rvalue;
%factor = 0.000005;
factor = 1000;
M = sim.dynamics.M;
Gn = sim.dynamics.Gn;
Gf = sim.dynamics.Gf; 
h = sim.dynamics.h;
FX = sim.dynamics.Forces;
MinvPext = M \ FX*h;
%PSI = sim.dynamics.PSI;
PSI = sim.dynamics.PSI;
NU = sim.dynamics.Vel;
nc = length(PSI);
U = sim.dynamics.U;
E = sim.dynamics.E; 
nd = size(Gf, 2);
nd = nd / nc;
% LCP_FIXED_POINT 
% This is the prox projection onto the positive plane using the pyramid 
% iteration with the Newton-Euler equation to make this a mixed LCP problem
MinvGn = M \ Gn;
MinvGf = M \ Gf;

A = [ Gn'*MinvGn   Gn'*MinvGf  zeros(nc)
      Gf'*MinvGn   Gf'*MinvGf  E
      U            -E'         zeros(nc)];
 
rn = factor / eigs(Gn' * MinvGn, 1);
rf = factor / eigs(Gf' * MinvGf, 1);
temp = 1/2 * (eigs(Gn' * MinvGn, 1) + eigs(Gf' * MinvGf, 1));
rs = factor / temp;
% parameters to be tuned
maxIter = 1000;   % the maximum number of iteration steps
% err = 1e5;       % The error
if isfield(sim.solver,'tolerance') && sim.solver.tolerance > 0
    toler = sim.solver.tolerance;
else
    toler = 1e-4;    % The tolerance of the error
end
pn = zeros(nc, 1);
pn_ellp1 = zeros(nc, 1);
pf = zeros(nc*nd, 1);
pf_ellp1 = zeros(nc*nd, 1);
s_ellp1 = zeros(nc, 1);
%pf_ellp1 = zeros(nc*nd, 1);
s  = zeros(nc, 1);  
% converge = zeros(maxIter, 1);
solution  = struct();
solution.total_error = zeros(maxIter, 1);
solution.normal_error = zeros(maxIter, 1); 
solution.friction_error = zeros(maxIter, 1) ;
solution.stick_or_slide = zeros(maxIter, 1);
solution.z = zeros(size(A, 2), maxIter);
solution.iterations = 0;
solution.direction_error = zeros(maxIter, 1);
solution.copositive_normal_error = zeros(maxIter, 1);
solution.copositive_friction_error = zeros(maxIter, 1);
solution.normal_neg_error = zeros(maxIter, 1);
solution.fric_neg_error = zeros(maxIter, 1);
 
%% converge the normal force and the frictional force
% converge the normal force first
for iter = 1 : maxIter  
    for CT = 1 : nc
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        pn_ellp1(CT, 1) = update_normal(pn(CT, 1), rn, PSI(CT, 1), h, Gn(:, CT), NU);
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        start_row = (CT-1)*nd + 1;
        end_row = CT*nd;
        pf_ellp1(start_row:end_row, 1) = update_fric(pf(start_row:end_row, 1), rf, Gf(:, start_row:end_row), NU, s(CT, 1), E(start_row:end_row, CT));
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
        s_ellp1(CT, 1)  = update_sliding(s(CT, 1), rs, U(CT, CT), pn_ellp1(CT, 1), pf_ellp1(start_row:end_row, 1), E(start_row:end_row, CT));
     
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        NU_ellp1 = update_vel(NU, MinvGn, pn_ellp1, MinvGf, pf_ellp1, MinvPext);
    end
   
    b        = setb(Gn, Gf, NU_ellp1, MinvPext, PSI, h, nc);
    z = [pn_ellp1; pf_ellp1; s_ellp1];
   % err = norm(z' * (A * z + b));
    
    pn = pn_ellp1;
    pf = pf_ellp1;
    s = s_ellp1;
    
    % get the magnitude of the frictional force 
%     pfMag = zeros(nc , 1);
%     resultant_f  = zeros(nc, 2);
%     theta = 2 * pi/nd;
%     for i = 1 : nc
%         for j = 1 : nd
%             resultant_f(i, 1) = resultant_f(i, 1) + pf((i-1)*nd+j, 1) * cos((j-1)*theta);
%             resultant_f(i, 2) = resultant_f(i, 2) + pf((i-1)*nd+j, 1) * sin((j-1)*theta);
%         end
%         pfMag(i, 1) = sqrt(resultant_f(i, 1)^2 + resultant_f(i, 2)^2);
%     end   
     solution = updateSolutionData(solution, iter, A, z, b, s, U, pn, PSI, pf, nd, Gf, NU_ellp1);
     
%     normErr = norm(PSI' * pn);
%     % s perp (U*pn - pf)
%     fricErr = norm(s' * (U * pn - pfMag));
%     totalErr = normErr + fricErr;
%     converge(iter, 1) = totalErr;
     
    if solution.total_error(iter) < toler
        break;
    end
 
end
 
end

function [NU_ellp1] = update_vel(NU, MinvGn, pn, MinvGf, pf, MinvPext)
  NU_ellp1 = NU + MinvGn * pn + MinvGf * pf + MinvPext;
end

function [pn_ellp1] = update_normal(pn, rn, PSI, h, Gn, NU_ellp1)
% Pn_ellp1 = prox(Pn - rn (PSI/h + Gn'*NU_ellp1))
    rhon = PSI/h + Gn'* NU_ellp1;
    pn_ellp1 = pn - rn*rhon;
% The normal force can not be negative, project onto the non negative space
    pn_ellp1(pn_ellp1<0) = 0;
end

function [pf_ellp1] = update_fric(pf, rf, Gf, NU_ellp1, s, E)
% Pf_ellp1 = prox(Pf - rf * (Gf * NU_ellp1 + s))
rhof = Gf' * NU_ellp1 + E * s;
pf_ellp1 = pf - rf * rhof;
pf_ellp1(pf_ellp1 < 0) = 0;
end

function [s_ellp1]  = update_sliding(s, rs, U, pn_ellp1, pf_ellp1, E)
rhos = U * pn_ellp1 - E' * pf_ellp1;
s_ellp1 = s - rs * rhos;
s_ellp1(s_ellp1 < 0) = 0;
end

function b = setb(Gn, Gf, NU, MinvPext, PSI, h, nc)
b = [ Gn'*(NU + MinvPext) + PSI/h;    % FX*h could be replaced if we stored Pext instead of Fext
      Gf'*(NU + MinvPext);
      zeros(nc,1) ];    
end

